/// <summary>
/// Reads axis joint information and rotation to the articulation body to produce the required motion
/// </summary>
/// <param name="joint">Structure containing joint information</param>
protected override void AdjustMovement(Joint joint)
{
axisofMotion = (joint.axis != null && joint.axis.xyz != null) ? joint.axis.xyz.ToVector3() : new Vector3(1, 0, 0);
unityJoint.linearLockX = ArticulationDofLock.LimitedMotion;
unityJoint.linearLockY = ArticulationDofLock.LockedMotion;
unityJoint.linearLockZ = ArticulationDofLock.LockedMotion;
unityJoint.twistLock = ArticulationDofLock.LimitedMotion;
Vector3 axisofMotionUnity = axisofMotion.Ros2Unity();
Quaternion Motion = new Quaternion();
Motion.SetFromToRotation(new Vector3(1, 0, 0), -1 * axisofMotionUnity);
unityJoint.anchorRotation = Motion;
if (joint.limit != null)
{
ArticulationDrive drive = unityJoint.xDrive;
drive.upperLimit = (float)(joint.limit.upper * Mathf.Rad2Deg);
drive.lowerLimit = (float)(joint.limit.lower * Mathf.Rad2Deg);
drive.forceLimit = (float)(joint.limit.effort);
unityJoint.maxAngularVelocity = (float)(joint.limit.velocity);
drive.damping = unityJoint.xDrive.damping;
drive.stiffness = unityJoint.xDrive.stiffness;
unityJoint.xDrive = drive;
}
}
void set_articulation_position(int idx, float position)
{
ArticulationDrive currentDrive = articulationChain[idx].xDrive;
currentDrive.target = position;
articulationChain[idx].xDrive = currentDrive;
}
private void ShowPrismaticLimits(ArticulationBody body, Matrix4x4 parentAnchorSpace, Vector3 anchorPosition, Quaternion anchorRotation)
{
Vector3 primaryAxis = Vector3.zero;
// compute the primary axis of the prismatic
ArticulationDrive drive = body.xDrive;
if (body.linearLockX != ArticulationDofLock.LockedMotion)
{
primaryAxis = Vector3.right;
drive = body.xDrive;
}
else if (body.linearLockY != ArticulationDofLock.LockedMotion)
{
primaryAxis = Vector3.up;
drive = body.yDrive;
}
else if (body.linearLockZ != ArticulationDofLock.LockedMotion)
{
primaryAxis = Vector3.forward;
drive = body.zDrive;
}
if (body.linearLockX == ArticulationDofLock.FreeMotion || body.linearLockY == ArticulationDofLock.FreeMotion || body.linearLockZ == ArticulationDofLock.FreeMotion)
{
DrawFreePrismatic(body, primaryAxis, anchorPosition, anchorRotation);
return;
}
DrawLimitedPrismatic(body, primaryAxis, drive, parentAnchorSpace);
}
void initJointsConfig()
{
foreach (var item in right_kinova.joints)
{
item.angularDamping = angular_friction;
item.linearDamping = linear_friction;
item.jointFriction = friction;
ArticulationDrive drive = item.xDrive;
drive.damping = damping;
drive.stiffness = stifness;
drive.targetVelocity = target_velocity;
item.xDrive = drive;
}
foreach (var item in left_kinova.joints)
{
item.angularDamping = angular_friction;
item.linearDamping = linear_friction;
item.jointFriction = friction;
ArticulationDrive drive = item.xDrive;
drive.damping = damping;
drive.stiffness = stifness;
drive.targetVelocity = target_velocity;
item.xDrive = drive;
}
}
/// <summary>
/// Reads axis joint information and rotation to the articulation body to produce the required motion
/// </summary>
/// <param name="joint">Structure containing joint information</param>
protected override void AdjustMovement(Joint joint) // Test this function
{
axisofMotion = (joint.axis != null && joint.axis.xyz != null) ? joint.axis.xyz.ToVector3() : new Vector3(1, 0, 0);
unityJoint.linearLockX = (joint.limit != null) ? ArticulationDofLock.LimitedMotion : ArticulationDofLock.FreeMotion;
unityJoint.linearLockY = ArticulationDofLock.LockedMotion;
unityJoint.linearLockZ = ArticulationDofLock.LockedMotion;
Vector3 axisofMotionUnity = axisofMotion.Ros2Unity();
Quaternion Motion = new Quaternion();
Motion.SetFromToRotation(new Vector3(1, 0, 0), axisofMotionUnity);
unityJoint.anchorRotation = Motion;
if (joint.limit != null)
{
ArticulationDrive drive = unityJoint.xDrive;
drive.upperLimit = (float)joint.limit.upper;
drive.lowerLimit = (float)joint.limit.lower;
drive.forceLimit = (float)joint.limit.effort;
#if UNITY_2020_2_OR_NEWER
unityJoint.maxLinearVelocity = (float)joint.limit.velocity;
#elif UNITY_2020_1
maxLinearVelocity = (float)joint.limit.velocity;
#endif
unityJoint.xDrive = drive;
}
}
// Start is called before the first frame update
void Start()
{
// start the ROS connection
ros = ROSConnection.instance;
// Set up the arm variables
this.gameObject.AddComponent <FKRobot>();
articulationChain = this.GetComponentsInChildren <ArticulationBody>(); // https://docs.unity3d.com/2020.1/Documentation/ScriptReference/ArticulationBody.html?_ga=2.54684075.1087433992.1613790814-228562203.1613145667
int defDyanmicVal = 10;
foreach (ArticulationBody joint in articulationChain)
{
// Set up each of the joints
joint.gameObject.AddComponent <JointControl>();
joint.jointFriction = defDyanmicVal;
joint.angularDamping = defDyanmicVal;
ArticulationDrive currentDrive = joint.xDrive;
currentDrive.forceLimit = forceLimit;
joint.xDrive = currentDrive;
print(joint);
}
num_joints = articulationChain.Length;
print(num_joints);
print("------------");
}
ArticulationDrive SetDriveLimits(ArticulationDrive drive, float lowerLimit, float upperLimit)
{
var tempDrive = drive;
tempDrive.lowerLimit = lowerLimit;
tempDrive.upperLimit = upperLimit;
return(tempDrive);
}
void FixedUpdate()
{
currentJoint = chain.gameObject.GetComponent <JointControl>().joint.xDrive;
currentJoint.target = GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[2];
currentJoint.stiffness = _controller.stiffness;
currentJoint.damping = _controller.damping;
chain.gameObject.GetComponent <JointControl>().joint.xDrive = currentJoint;
}
/// <summary>
/// Rotates the joint by deltaState m
/// </summary>
/// <param name="deltaState">amount in m by which joint needs to be rotated</param>
protected override void OnUpdateJointState(float deltaState)
{
#if UNITY_2020_1_OR_NEWER
ArticulationDrive drive = unityJoint.xDrive;
drive.target += deltaState;
unityJoint.xDrive = drive;
#else
transform.Translate(unityJoint.axis * deltaState);
#endif
}
/// <summary>
/// Rotates the joint by deltaState radians
/// </summary>
/// <param name="deltaState">amount in radians by which joint needs to be rotated</param>
protected override void OnUpdateJointState(float deltaState)
{
#if UNITY_2020_1_OR_NEWER
ArticulationDrive drive = unityJoint.xDrive;
drive.target += deltaState * Mathf.Rad2Deg;
unityJoint.xDrive = drive;
#else
Quaternion rot = Quaternion.AngleAxis(-deltaState * Mathf.Rad2Deg, unityJoint.axis);
transform.rotation = transform.rotation * rot;
#endif
}
public void UpdateControlType(JointControl joint)
{
joint.controltype = control;
if (control == ControlType.PositionControl)
{
ArticulationDrive drive = joint.joint.xDrive;
drive.stiffness = stiffness;
drive.damping = damping;
joint.joint.xDrive = drive;
}
}
void FixedUpdate()
{
var jPos = body.jointPosition;
jointPosition = new Vector3(jPos[0], jPos[1], jPos[2]);
jointPosition *= Mathf.Rad2Deg;
if (jointMode == JointMode.useDrive)
{
// Normalize Quaternion to suppress greater than 360 degree rotations
Quaternion normalizedRotation = Quaternion.Normalize(transform.rotation);
// Calculate drive targets
Vector3 driveTarget = new Vector3(
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.x),
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.y),
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.z));
// Copy the X, Y, and Z target values into the drives...
ArticulationDrive xDrive = body.xDrive; xDrive.target = driveTarget.x; body.xDrive = xDrive;
ArticulationDrive yDrive = body.yDrive; yDrive.target = driveTarget.y; body.yDrive = yDrive;
ArticulationDrive zDrive = body.zDrive; zDrive.target = driveTarget.z; body.zDrive = zDrive;
}
else
{
Quaternion deltaRotation = Quaternion.Normalize(Quaternion.Inverse(body.transform.localRotation) * transform.rotation);
// Calculate drive velocity necessary to undo this delta in one fixed timestep
ArticulationReducedSpace driveTargetForce = new ArticulationReducedSpace(
((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.x) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength,
((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.y) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength,
((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.z) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength);
// Apply the force in local space (unlike AddTorque which is global space)
// Ideally we'd use inverse dynamics or jointVelocity, but jointVelocity is bugged in 2020.1a22
if (jointMode == JointMode.useJointForce)
{
body.jointForce = driveTargetForce;
}
else if (jointMode == JointMode.useJointVelocity)
{
body.jointVelocity = driveTargetForce;
}
else if (jointMode == JointMode.useJointPosition)
{
Quaternion normalizedRotation = Quaternion.Normalize(transform.rotation);
ArticulationReducedSpace driveTarget = new ArticulationReducedSpace(
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.x) * Mathf.Deg2Rad,
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.y) * Mathf.Deg2Rad,
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.z) * Mathf.Deg2Rad);
body.jointPosition = driveTarget;
}
}
}
public void Initialize()
{
m_Body = GetComponent <ArticulationBody>();
m_Drive = m_Body.xDrive;
FindConnectedParent(transform.parent);
m_DefRot = CrntRot;
m_Min = m_Drive.lowerLimit;
m_Max = m_Drive.upperLimit;
m_Ext = (m_Max - m_Min) * 0.5f;
m_Mid = m_Min + m_Ext;
}
protected override Joint.Limit ExportLimitData()
{
#if UNITY_2020_1_OR_NEWER
ArticulationDrive drive = GetComponent <ArticulationBody>().yDrive;
return(new Joint.Limit(drive.lowerLimit, drive.upperLimit, EffortLimit, VelocityLimit));
#else
ConfigurableJoint configurableJoint = (ConfigurableJoint)unityJoint;
return(new Joint.Limit(
Math.Round(-configurableJoint.linearLimit.limit, RoundDigits),
Math.Round(configurableJoint.linearLimit.limit, RoundDigits),
EffortLimit, VelocityLimit));
#endif
}
protected override Joint.Limit ExportLimitData()
{
#if UNITY_2020_1_OR_NEWER
ArticulationDrive drive = GetComponent <ArticulationBody>().xDrive;
return(new Joint.Limit(drive.lowerLimit, drive.upperLimit, drive.forceLimit, unityJoint.maxLinearVelocity));
#else
PrismaticJointLimitsManager prismaticLimits = GetComponent <PrismaticJointLimitsManager>();
return(new Joint.Limit(
Math.Round(prismaticLimits.PositionLimitMin, RoundDigits),
Math.Round(prismaticLimits.PositionLimitMax, RoundDigits),
EffortLimit,
VelocityLimit));
#endif
}
protected override void AdjustMovement(Joint joint)
{
if (joint.axis == null || joint.axis.xyz == null)
{
joint.axis = new Joint.Axis(new double[] { 1, 0, 0 });
}
axisofMotion = new Vector3((float)joint.axis.xyz[0], (float)joint.axis.xyz[1], (float)joint.axis.xyz[2]);
int motionAxis = joint.axis.AxisofMotion();
Quaternion motion = unityJoint.anchorRotation;
unityJoint.linearLockX = ArticulationDofLock.LockedMotion;
if (joint.limit != null)
{
unityJoint.linearLockY = ArticulationDofLock.LimitedMotion;
unityJoint.linearLockZ = ArticulationDofLock.LimitedMotion;
var drive = new ArticulationDrive()
{
stiffness = unityJoint.xDrive.stiffness,
damping = unityJoint.xDrive.damping,
forceLimit = (float)joint.limit.effort,
lowerLimit = (float)joint.limit.lower,
upperLimit = (float)joint.limit.upper,
};
unityJoint.xDrive = drive;
unityJoint.zDrive = drive;
unityJoint.yDrive = drive;
unityJoint.maxLinearVelocity = (float)joint.limit.velocity;
}
else
{
unityJoint.linearLockZ = ArticulationDofLock.FreeMotion;
unityJoint.linearLockY = ArticulationDofLock.FreeMotion;
}
switch (motionAxis)
{
case 0:
motion.eulerAngles = new Vector3(0, -90, 0);
break;
case 1:
motion.eulerAngles = new Vector3(0, 0, 0);
break;
case 2:
motion.eulerAngles = new Vector3(0, 0, 90);
break;
}
unityJoint.anchorRotation = motion;
}
protected override Joint.Limit ExportLimitData()
{
#if UNITY_2020_1_OR_NEWER
ArticulationDrive drive = unityJoint.xDrive;
return(new Joint.Limit(drive.lowerLimit * Mathf.Deg2Rad, drive.upperLimit * Mathf.Deg2Rad, drive.forceLimit, unityJoint.maxAngularVelocity));
#else
HingeJointLimitsManager hingeJointLimits = GetComponent <HingeJointLimitsManager>();
return(new Joint.Limit(
Math.Round(hingeJointLimits.LargeAngleLimitMin * Mathf.Deg2Rad, RoundDigits),
Math.Round(hingeJointLimits.LargeAngleLimitMax * Mathf.Deg2Rad, RoundDigits),
EffortLimit,
VelocityLimit));
#endif
}
private void ClampDriveLimits(SerializedProperty drive, ArticulationDrive bodyDrive)
{
var lowerLimit = drive.FindPropertyRelative("lowerLimit");
var upperLimit = drive.FindPropertyRelative("upperLimit");
if (bodyDrive.lowerLimit != lowerLimit.floatValue)
{
lowerLimit.floatValue = Mathf.Min(lowerLimit.floatValue, upperLimit.floatValue);
}
if (bodyDrive.upperLimit != upperLimit.floatValue)
{
upperLimit.floatValue = Mathf.Max(lowerLimit.floatValue, upperLimit.floatValue);
}
}
private void ShowPrismaticLimits(ArticulationBody body, ArticulationBody parentBody, Matrix4x4 parentAnchorSpace)
{
// if prismatic and unlocked - nothing to visualise
if (body.linearLockX == ArticulationDofLock.FreeMotion || body.linearLockY == ArticulationDofLock.FreeMotion || body.linearLockZ == ArticulationDofLock.FreeMotion)
{
return;
}
float dashSize = 5;
// compute the primary axis of the prismatic
Vector3 primaryAxis = Vector3.zero;
ArticulationDrive drive = body.xDrive;
if (body.linearLockX == ArticulationDofLock.LimitedMotion)
{
primaryAxis = Vector3.right;
drive = body.xDrive;
}
else if (body.linearLockY == ArticulationDofLock.LimitedMotion)
{
primaryAxis = Vector3.up;
drive = body.yDrive;
}
else if (body.linearLockZ == ArticulationDofLock.LimitedMotion)
{
primaryAxis = Vector3.forward;
drive = body.zDrive;
}
// now show the valid movement along the axis as well as limits
using (new Handles.DrawingScope(parentAnchorSpace))
{
Vector3 lowerPoint = primaryAxis * drive.lowerLimit;
Vector3 upperPoint = primaryAxis * drive.upperLimit;
Quaternion orientation = Quaternion.LookRotation(primaryAxis);
Handles.color = Color.red;
Handles.CylinderHandleCap(0, lowerPoint, orientation, CapScale, EventType.Repaint);
Handles.color = Color.green;
Handles.CylinderHandleCap(0, upperPoint, orientation, CapScale, EventType.Repaint);
Handles.color = Color.white;
Handles.DrawDottedLine(lowerPoint, upperPoint, dashSize);
}
}
// private float left_inner_finger_offset = -41.5f;
// private float left_inner_knuckle_offset = 131.5f;
// private float right_outer_knuckle_offset = -48.46f;
// private float right_inner_finger_offset = -41.5f;
// private float right_inner_knuckle_offset = -48.46f;
IEnumerator EngageForceAfterInit()
{
print("Start waiting");
yield return(new WaitForSeconds(2));
int defDyanmicVal = 10;
foreach (ArticulationBody joint in articulationChain)
{
// Set up each of the joints
joint.gameObject.AddComponent <JointControl>();
joint.jointFriction = defDyanmicVal;
joint.angularDamping = defDyanmicVal;
ArticulationDrive currentDrive = joint.xDrive;
currentDrive.forceLimit = forceLimit;
joint.xDrive = currentDrive;
}
}
void Start()
{
previousIndex = selectedIndex = 1;
this.gameObject.AddComponent <FKRobot>();
articulationChain = this.GetComponentsInChildren <ArticulationBody>();
int defDyanmicVal = 10;
foreach (ArticulationBody joint in articulationChain)
{
joint.gameObject.AddComponent <JointControl>();
joint.jointFriction = defDyanmicVal;
joint.angularDamping = defDyanmicVal;
ArticulationDrive currentDrive = joint.xDrive;
currentDrive.forceLimit = forceLimit;
joint.xDrive = currentDrive;
}
DisplaySelectedJoint(selectedIndex);
StoreJointColors(selectedIndex);
}
// #################################### Setters ######################################
// TODO: Gripper - this should be a subscriber which just sets joint positions whenever it gets a joint command msg
void commandJointPositions(List <float> positions)
{
// Update the joint positions from a float array.
// 0 - base (controls nothing)
// 1 - shoulder
// 2 - upper arm
// 3 - forearm
// 4 - wrist 1
// 5 - wrist 2
// 6 - wrist 3
// 7 - ee link
foreach (int idx in Range(0, positions.Count))
{
ArticulationDrive currentDrive = articulationChain[idx].xDrive;
currentDrive.target = positions[idx];
articulationChain[idx].xDrive = currentDrive;
}
print($"{positions[11]} {positions[13]} {positions[15]}");
}
void FixedUpdate()
{
speed = controller.speed;
torque = controller.torque;
acceleration = controller.acceleration;
if (joint.jointType != ArticulationJointType.FixedJoint)
{
if (controltype == RosSharp.Control.ControlType.PositionControl)
{
ArticulationDrive currentDrive = joint.xDrive;
float newTargetDelta = (int)direction * Time.fixedDeltaTime * speed;
if (newTargetDelta + currentDrive.target <= currentDrive.upperLimit && newTargetDelta + currentDrive.target >= currentDrive.lowerLimit)
{
currentDrive.target += newTargetDelta;
}
joint.xDrive = currentDrive;
}
}
}
protected override void AdjustMovement(Joint joint)
{
axisofMotion = (joint.axis == null || joint.axis.xyz == null) ? new Vector3(1, 0, 0) : new Vector3((float)joint.axis.xyz[0], (float)joint.axis.xyz[1], (float)joint.axis.xyz[2]);
int motionAxis = joint.axis.AxisofMotion();
Quaternion motion = unityJoint.anchorRotation;
unityJoint.linearLockX = ArticulationDofLock.LockedMotion;
if (joint.limit != null)
{
unityJoint.linearLockY = ArticulationDofLock.LimitedMotion;
unityJoint.linearLockZ = ArticulationDofLock.LimitedMotion;
ArticulationDrive drive = unityJoint.xDrive;
drive.upperLimit = (float)joint.limit.upper;
drive.lowerLimit = (float)joint.limit.lower;
unityJoint.zDrive = drive;
unityJoint.yDrive = drive;
}
else
{
unityJoint.linearLockZ = ArticulationDofLock.FreeMotion;
unityJoint.linearLockY = ArticulationDofLock.FreeMotion;
}
switch (motionAxis)
{
case 0:
motion.eulerAngles = new Vector3(0, -90, 0);
break;
case 1:
motion.eulerAngles = new Vector3(0, 0, 0);
break;
case 2:
motion.eulerAngles = new Vector3(0, 0, 90);
break;
}
unityJoint.anchorRotation = motion;
}
void FixedUpdate()
{
currentJoint = chain.gameObject.GetComponent <JointControl>().joint.xDrive;
if (joint == 1)
{
currentJoint.target = GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[0];
}
if (joint == 2)
{
currentJoint.target = GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[1];
}
if (joint == 3)
{
currentJoint.target = GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[2];
}
if (joint == 4)
{
currentJoint.target = GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[3];
}
if (joint == 5)
{
currentJoint.target = GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[4];
}
if (joint == 6)
{
currentJoint.target = GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[5];
}
currentJoint.stiffness = _controller.stiffness;
currentJoint.damping = _controller.damping;
chain.gameObject.GetComponent <JointControl>().joint.xDrive = currentJoint;
Debug.Log("value" + GlobalVariables_TCP_IP_client.robotBaseRotLink_UR3_j[0]);
}
private void DrawLimitedPrismatic(ArticulationBody body, Vector3 primaryAxis, ArticulationDrive drive, Matrix4x4 parentAnchorSpace)
{
using (new Handles.DrawingScope(parentAnchorSpace))
{
Vector3 lowerPoint = primaryAxis * drive.lowerLimit;
Vector3 upperPoint = primaryAxis * drive.upperLimit;
Handles.DrawDottedLine(lowerPoint, upperPoint, DashSize);
int idLower = GUIUtility.GetControlID(Handles.s_SliderHash, FocusType.Passive);
int idUpper = GUIUtility.GetControlID(Handles.s_SliderHash, FocusType.Passive);
EditorGUI.BeginChangeCheck();
{
Handles.color = Handles.xAxisColor;
lowerPoint = Handles.Slider(idLower, lowerPoint, primaryAxis, CapScale, prismaticHandleDrawFunction, 0);
Handles.color = Handles.yAxisColor;
upperPoint = Handles.Slider(idUpper, upperPoint, primaryAxis, CapScale, prismaticHandleDrawFunction, 0);
}
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(target, "Changing Articulation body parent anchor prismatic limits");
float newLowerLimit = drive.lowerLimit;
float newUpperLimit = drive.upperLimit;
// Disallow moving Lower limit past Upper limit and vice versa, based on which handle is being held down
if (GUIUtility.hotControl == idLower)
{
float directionLower = Mathf.Sign(lowerPoint.x + lowerPoint.y + lowerPoint.z);
newLowerLimit = lowerPoint.magnitude * directionLower;
if (newLowerLimit > drive.upperLimit)
{
newLowerLimit = drive.upperLimit;
}
}
else if (GUIUtility.hotControl == idUpper)
{
float directionUpper = Mathf.Sign(upperPoint.x + upperPoint.y + upperPoint.z);
newUpperLimit = upperPoint.magnitude * directionUpper;
if (newUpperLimit < drive.lowerLimit)
{
newUpperLimit = drive.lowerLimit;
}
}
ArticulationDrive tempDrive = SetDriveLimits(drive, newLowerLimit, newUpperLimit);
if (body.linearLockX == ArticulationDofLock.LimitedMotion)
{
body.xDrive = tempDrive;
}
else if (body.linearLockY == ArticulationDofLock.LimitedMotion)
{
body.yDrive = tempDrive;
}
else if (body.linearLockZ == ArticulationDofLock.LimitedMotion)
{
body.zDrive = tempDrive;
}
}
}
}
/// <summary>
/// Create a prefab from a .urdf file.
/// </summary>
/// <param name="urdfPath">The path to the .urdf file.</param>
/// <param name="settings">Import settings for ROS.</param>
/// <param name="immovable">If true, the root object is immovable.</param>
private static void CreatePrefab(string urdfPath, ImportSettings settings, bool immovable)
{
// Import the robot.
UrdfRobotExtensions.Create(urdfPath, settings);
// Remove irrelevant ROS components.
GameObject robot = Object.FindObjectOfType <UrdfRobot>().gameObject;
robot.DestroyAll <UrdfJoint>();
robot.DestroyAll <UrdfInertial>();
robot.DestroyAll <UrdfVisuals>();
robot.DestroyAll <UrdfVisual>();
robot.DestroyAll <UrdfCollisions>();
robot.DestroyAll <UrdfCollision>();
robot.DestroyAll <UrdfLink>();
robot.DestroyAll <UrdfRobot>();
robot.DestroyAll <Controller>();
// Destroy unwanted objects.
robot.DestroyAllComponents <Light>();
robot.DestroyAllComponents <Camera>();
robot.DestroyAllComponents <UrdfPlugins>();
// Save the generated collider meshes.
string collidersDirectory = Path.Combine(Application.dataPath, "collider_meshes", robot.name);
if (!Directory.Exists(collidersDirectory))
{
Directory.CreateDirectory(collidersDirectory);
}
foreach (MeshCollider mc in robot.GetComponentsInChildren <MeshCollider>())
{
mc.sharedMesh.name = Random.Range(0, 1000000).ToString();
AssetDatabase.CreateAsset(mc.sharedMesh, "Assets/collider_meshes/" + robot.name + "/" + mc.sharedMesh.name);
AssetDatabase.SaveAssets();
}
// Load the .urdf file.
XmlDocument doc = new XmlDocument();
doc.Load(urdfPath);
XmlNode xmlRoot = doc.SelectSingleNode("robot");
// Get the prismatic joints.
XmlNodeList jointNodes = xmlRoot.SelectNodes("joint");
// The name of each prismatic joint and its axis.
Dictionary <string, DriveAxis> prismaticAxes = new Dictionary <string, DriveAxis>();
for (int i = 0; i < jointNodes.Count; i++)
{
string jointType = jointNodes[i].Attributes["type"].Value.ToLower();
if (jointType == "prismatic")
{
string jointChild = jointNodes[i].SelectSingleNode("child").Attributes["link"].Value;
if (prismaticAxes.ContainsKey(jointChild))
{
continue;
}
Vector3 xyz = jointNodes[i].SelectSingleNode("axis").Attributes["xyz"].Value.xyzToVector3();
DriveAxis driveAxis;
// Get the drive axis for a single-axis rotation.
if (xyz.x != 0)
{
driveAxis = DriveAxis.x;
}
else if (xyz.y != 0)
{
driveAxis = DriveAxis.y;
}
else if (xyz.z != 0)
{
driveAxis = DriveAxis.z;
}
else
{
throw new System.Exception("No axis for: " + jointChild);
}
prismaticAxes.Add(jointChild, driveAxis);
}
}
// Fix the articulation drives.;
foreach (ArticulationBody a in robot.GetComponentsInChildren <ArticulationBody>())
{
if (a.jointType == ArticulationJointType.RevoluteJoint)
{
ArticulationDrive drive = a.xDrive;
drive.stiffness = 1000;
drive.damping = 180;
a.xDrive = drive;
}
// Set the prismatic joint's drive values and expected axis.
else if (a.jointType == ArticulationJointType.PrismaticJoint)
{
DriveAxis da = prismaticAxes[a.name];
ArticulationDrive drive;
if (da == DriveAxis.x)
{
drive = a.xDrive;
}
else if (da == DriveAxis.y)
{
drive = a.yDrive;
}
else
{
drive = a.zDrive;
}
drive.forceLimit = a.xDrive.forceLimit;
drive.stiffness = 1000;
drive.damping = 180;
drive.lowerLimit = a.xDrive.lowerLimit;
drive.upperLimit = a.xDrive.upperLimit;
if (da == DriveAxis.x)
{
a.linearLockX = ArticulationDofLock.LimitedMotion;
a.linearLockY = ArticulationDofLock.LockedMotion;
a.linearLockZ = ArticulationDofLock.LockedMotion;
a.xDrive = drive;
}
else if (da == DriveAxis.y)
{
a.linearLockX = ArticulationDofLock.LockedMotion;
a.linearLockY = ArticulationDofLock.LimitedMotion;
a.linearLockZ = ArticulationDofLock.LockedMotion;
a.yDrive = drive;
}
else
{
a.linearLockX = ArticulationDofLock.LockedMotion;
a.linearLockY = ArticulationDofLock.LockedMotion;
a.linearLockZ = ArticulationDofLock.LimitedMotion;
a.zDrive = drive;
}
ExpectedDriveAxis eda = a.gameObject.AddComponent <ExpectedDriveAxis>();
eda.axis = da;
}
a.anchorPosition = Vector3.zero;
// Set the root articulation body as the root object.
if (a.isRoot)
{
a.immovable = immovable;
}
}
// Destroy redundant ArticulationBodies.
ArticulationBody[] badBodies = robot.GetComponentsInChildren <ArticulationBody>().
Where(a => !a.isRoot && a.mass < 0.01f && a.jointType == ArticulationJointType.FixedJoint &&
a.GetComponentsInChildren <MeshFilter>().Length == 0 && a.GetComponentsInChildren <MeshCollider>().Length == 0).
ToArray();
foreach (ArticulationBody b in badBodies)
{
Object.DestroyImmediate(b.gameObject);
}
// Get the root object.
if (robot.GetComponent <ArticulationBody>() == null)
{
ArticulationBody[] rootBodies = Object.FindObjectsOfType <ArticulationBody>().Where(a => a.isRoot).ToArray();
if (rootBodies.Length != 1)
{
Debug.LogError("Root object of the robot doesn't have an ArticulationBody.");
}
else
{
rootBodies[0].gameObject.transform.parent = null;
rootBodies[0].gameObject.name = robot.name;
Object.DestroyImmediate(robot);
robot = rootBodies[0].gameObject;
}
}
// Create the prefab directory if it doesn't exist.
string absolutePrefabPath = Path.Combine(Application.dataPath, "prefabs");
if (!Directory.Exists(absolutePrefabPath))
{
Directory.CreateDirectory(absolutePrefabPath);
}
// Create the prefab.
PrefabUtility.SaveAsPrefabAsset(robot, "Assets/prefabs/" + robot.name + ".prefab");
Object.DestroyImmediate(robot);
}
// Start is called before the first frame update
void Start()
{
List <InputDevice> devices = new List <InputDevice>();
InputDevices.GetDevices(devices);
foreach (var item in devices)
{
Debug.Log(item.name + item.characteristics);
}
// start the ROS connection
ros = ROSConnection.instance;
DateTime foo = DateTime.Now;
restartTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();
// Set up a subscriber to listen to commands
ROSConnection.instance.Subscribe <JointPositions>("joint_commands", executeCommand);
//
ROSConnection.instance.Subscribe <AchievedGoal>("reset_environment", resetEnvironment);
// Set up the arm variables
this.gameObject.AddComponent <FKRobot>();
articulationChain = this.GetComponentsInChildren <ArticulationBody>(); // https://docs.unity3d.com/2020.1/Documentation/ScriptReference/ArticulationBody.html?_ga=2.54684075.1087433992.1613790814-228562203.1613145667
int defDyanmicVal = 10;
foreach (ArticulationBody joint in articulationChain)
{
// Set up each of the joints
joint.gameObject.AddComponent <JointControl>();
joint.jointFriction = defDyanmicVal;
joint.angularDamping = defDyanmicVal;
ArticulationDrive currentDrive = joint.xDrive;
currentDrive.forceLimit = 10;
joint.xDrive = currentDrive;
print(joint);
}
EngageForceAfterInit();
num_joints = articulationChain.Length;
// Rendering stuff
if (OVRManager.isHmdPresent & useVR)
{
print("VR Active");
}
else
{
// Make it so the VR rig just acts like any old camera
Vector3 temp = new Vector3(-0.3f, -0.4f, -0.1f);
cameraTransform.transform.position = temp;
Vector3 rot = new Vector3(20f, 30f, 0f);
cameraTransform.transform.rotation = Quaternion.Euler(rot);
}
shoulderRenderTexture = new RenderTexture(256, 256, 24, UnityEngine.Experimental.Rendering.GraphicsFormat.R8G8B8A8_SRGB); // this is very important
shoulderRenderTexture.Create();
gripperRenderTexture = new RenderTexture(64, 64, 24, UnityEngine.Experimental.Rendering.GraphicsFormat.R8G8B8A8_SRGB); // this is very important
gripperRenderTexture.Create();
reset_to_default();
}
void FixedUpdate()
{
speed = controller.speed;
torque = controller.torque;
acceleration = controller.acceleration;
if (joint.jointType != ArticulationJointType.FixedJoint)
{
if (controltype == RosSharp.Control.ControlType.PositionControl)
{
ArticulationDrive currentDrive = joint.xDrive;
float newTargetDelta = (int)direction * Time.fixedDeltaTime * speed;
if (joint.jointType == ArticulationJointType.RevoluteJoint)
{
if (joint.twistLock == ArticulationDofLock.LimitedMotion)
{
if (newTargetDelta + currentDrive.target > currentDrive.upperLimit)
{
currentDrive.target = currentDrive.upperLimit;
}
else if (newTargetDelta + currentDrive.target < currentDrive.lowerLimit)
{
currentDrive.target = currentDrive.lowerLimit;
}
else
{
currentDrive.target += newTargetDelta;
}
}
else
{
currentDrive.target += newTargetDelta;
}
}
else if (joint.jointType == ArticulationJointType.PrismaticJoint)
{
if (joint.linearLockX == ArticulationDofLock.LimitedMotion)
{
if (newTargetDelta + currentDrive.target > currentDrive.upperLimit)
{
currentDrive.target = currentDrive.upperLimit;
}
else if (newTargetDelta + currentDrive.target < currentDrive.lowerLimit)
{
currentDrive.target = currentDrive.lowerLimit;
}
else
{
currentDrive.target += newTargetDelta;
}
}
else
{
currentDrive.target += newTargetDelta;
}
}
joint.xDrive = currentDrive;
}
}
}
private void ShowAngularLimitSpan(bool freeMotion, ArticulationBody body, ArticulationBody parentBody, Color color, Vector3 axis, Matrix4x4 space, ArticulationDrive drive, Vector3 zeroDirection)
{
// check if it's a free span - show only a solid disc in this case
if (freeMotion)
{
using (new Handles.DrawingScope(space))
{
color.a = 0.3f;
Handles.color = color;
Handles.DrawSolidDisc(Vector3.zero, axis, GizmoLinearSize);
}
return;
}
// here we know the angle is limited - show a span
float totalAngle = drive.upperLimit - drive.lowerLimit;
Quaternion zeroPose = Quaternion.FromToRotation(Vector3.forward, Vector3.Cross(axis, zeroDirection));
Quaternion lowerRotation = Quaternion.AngleAxis(drive.lowerLimit, axis);
Quaternion upperRotation = Quaternion.AngleAxis(drive.upperLimit, axis);
Vector3 from = lowerRotation * zeroDirection;
Vector3 to = upperRotation * zeroDirection;
// Nb: Cylinder cap is oriented along Z
using (new Handles.DrawingScope(space))
{
color.a = 0.3f;
Handles.color = color;
Handles.DrawSolidArc(Vector3.zero, axis, from, totalAngle, GizmoLinearSize);
Handles.color = Color.red;
Handles.CylinderHandleCap(0, from * GizmoLinearSize, lowerRotation * zeroPose, CapScale, EventType.Repaint);
Handles.color = Color.green;
Handles.CylinderHandleCap(0, to * GizmoLinearSize, upperRotation * zeroPose, CapScale, EventType.Repaint);
}
}