private void AddGripper(ref Vector3 position)
{
var upperShape = new BoxShape(new BoxShapeProperties(
"wrist", // name
100, // mass
new Pose(new Vector3(0, GripperHeight / 2f, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(GripperWidth, GripperHeight, GripperDepth) // dimension
));
position = new Vector3(0, WristHeight-WristDepth/2f+0.009f, 0);
_gripperEntity = new SingleShapeSegmentEntity(upperShape, position);
_gripperEntity.State.Name = this.State.Name + " Gripper";
_gripperEntity.State.Flags |= EntitySimulationModifiers.DisableCollisions;
_gripperEntity.State.Assets.Mesh = @"RobotArm_Gripper.obj";
var jointAngularProperties = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(90), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-90), 0, new SpringProperties()),
};
var connectors = new[]
{
new EntityJointConnector(_gripperEntity, new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_wristEntity, new Vector3(1, 0, 0), new Vector3(0, 1, 0), position)
};
_gripperEntity.CustomJoint = new Joint();
_gripperEntity.CustomJoint.State = new JointProperties(jointAngularProperties, connectors);
_gripperEntity.CustomJoint.State.Name = "WristRotate|-90|90|";
_gripperEntity.Parent = _wristEntity;
_wristEntity.InsertEntity(_gripperEntity);
_joints.Add(new JointDesc(_gripperEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(jointAngularProperties.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(jointAngularProperties.UpperTwistLimit.LimitThreshold),
_gripperEntity, true));
//TODO: temporary
_joints.Add(new JointDesc("Grip", 0, 2f, _gripperEntity, false));
}
private void AddLowerArm(ref Vector3 position)
{
var lowerShape = new BoxShape(new BoxShapeProperties(
"lower arm", // name
100, // mass
new Pose(new Vector3(0, LowerHeight / 2f - LowerDepth/2f, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(LowerWidth, LowerHeight, LowerDepth) // dimension
));
position = TopLowerJointOffset;
_lowerEntity = new SingleShapeSegmentEntity(lowerShape, position);
_lowerEntity.State.Name = this.State.Name + " Lower Arm";
_lowerEntity.State.Flags |= EntitySimulationModifiers.DisableCollisions;
_lowerEntity.State.Assets.Mesh = @"RobotArm_Lower.obj";
//_lowerEntity.MeshTranslation = new Vector3(-LowerWidth/2f, -0.012f, -LowerDepth/2f);
var jointAngularProperties = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, Conversions.DegreesToRadians(52)), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(54.1f), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-90.1f), 0, new SpringProperties()),
};
var connectors = new[]
{
new EntityJointConnector(_lowerEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_topEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), TopLowerJointOffset)
};
_lowerEntity.CustomJoint = new Joint();
_lowerEntity.CustomJoint.State = new JointProperties(jointAngularProperties, connectors);
_lowerEntity.CustomJoint.State.Name = "Shoulder|-90|54|";
_lowerEntity.Parent = _topEntity;
_topEntity.InsertEntity(_lowerEntity);
_joints.Add(new JointDesc(_lowerEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(jointAngularProperties.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(jointAngularProperties.UpperTwistLimit.LimitThreshold),
_lowerEntity, false));
}
private void AddUpperArm(ref Vector3 position)
{
var upperShape = new BoxShape(new BoxShapeProperties(
"upper", // name
100, // mass
new Pose(new Vector3(0, UpperHeight/2f - UpperBottomJointOffset, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(UpperWidth, UpperHeight, UpperDepth) // dimension
));
position = LowerUpperJointOffset;
_upperEntity = new SingleShapeSegmentEntity(upperShape, position);
_upperEntity.State.Name = this.State.Name + " Upper";
//_upperEntity.State.Flags |= EntitySimulationModifiers.DisableCollisions;
_upperEntity.State.Assets.Mesh = @"RobotArm_Upper.obj";
var jointAngularProperties = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(155), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-155), 0, new SpringProperties()),
};
var connectors = new[]
{
new EntityJointConnector(_upperEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_lowerEntity, new Vector3(0, 1, 0), new Vector3(1, 0, 0), LowerUpperJointOffset)
};
_upperEntity.CustomJoint = new Joint();
_upperEntity.CustomJoint.State = new JointProperties(jointAngularProperties, connectors);
_upperEntity.CustomJoint.State.Name = "Elbow|-155|155|";
_upperEntity.Parent = _lowerEntity;
_lowerEntity.InsertEntity(_upperEntity);
_joints.Add(new JointDesc(_upperEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(jointAngularProperties.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(jointAngularProperties.UpperTwistLimit.LimitThreshold),
_upperEntity, false));
}
private void AddTop(ref Vector3 position)
{
var topShape = new BoxShape(new BoxShapeProperties(
"top", // name
100, // mass
new Pose(new Vector3(0, TopHeight/2, 0), new Quaternion(0, 0, 0, 1)),
new Vector3(TopRadius*2, TopHeight, TopRadius*2)
));
position += new Vector3(0, BaseHeight + TopGap, 0);
_topEntity = new SingleShapeSegmentEntity(topShape, position);
_topEntity.State.Name = this.State.Name + " Top";
_topEntity.State.Assets.Mesh = @"RobotArm_Top.obj";
_topEntity.Parent = _baseEntity;
var topAngular = new JointAngularProperties
{
TwistMode = JointDOFMode.Limited,
TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000),
UpperTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(90.1f), 0, new SpringProperties()),
LowerTwistLimit = new JointLimitProperties(Conversions.DegreesToRadians(-90.1f), 0, new SpringProperties()),
};
var topConnector = new[]
{
new EntityJointConnector(_topEntity, new Vector3(1, 0, 0), new Vector3(0, -1, 0), new Vector3(0, 0, 0)),
new EntityJointConnector(_baseEntity, new Vector3(1, 0, 0), new Vector3(0, -1, 0), new Vector3(0, BaseHeight + TopGap, 0))
};
_topEntity.CustomJoint = new Joint();
_topEntity.CustomJoint.State = new JointProperties(topAngular, topConnector);
_topEntity.CustomJoint.State.Name = "BaseJoint|-90|90|";
// add as child of base entity
_baseEntity.InsertEntity(_topEntity);
_joints.Add(new JointDesc(_topEntity.CustomJoint.State.Name,
Conversions.RadiansToDegrees(topAngular.LowerTwistLimit.LimitThreshold),
Conversions.RadiansToDegrees(topAngular.UpperTwistLimit.LimitThreshold),
_topEntity, true));
}
