public void SetPublishJointStates(bool publish)
{
if (publish)
{
JointStatePublisher jointStatePublisher = transform.AddComponentIfNotExists <JointStatePublisher>();
jointStatePublisher.JointStateReaders = new List <JointStateReader>();
foreach (UrdfJoint urdfJoint in UrdfRobot.GetComponentsInChildren <UrdfJoint>())
{
if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
{
jointStatePublisher.JointStateReaders.Add(urdfJoint.transform.AddComponentIfNotExists <JointStateReader>());
}
}
}
else
{
GetComponent <JointStatePublisher>()?.JointStateReaders.Clear();
foreach (JointStateReader reader in UrdfRobot.GetComponentsInChildren <JointStateReader>())
{
reader.transform.DestroyImmediateIfExists <JointStateReader>();
}
}
}
public void SetSubscribeTrajectoryPositions(bool subscribe)
{
if (subscribe)
{
TrajectoryPositionsSubscriber trajectoryPositionsSubscriber = transform.AddComponentIfNotExists <TrajectoryPositionsSubscriber>();
trajectoryPositionsSubscriber.trajectoryPositionsWriters = new List <TrajectoryPositionsWriter>();
trajectoryPositionsSubscriber.jointNames = new List <string>();
foreach (UrdfJoint urdfJoint in ghostRobot.GetComponentsInChildren <UrdfJoint>())
{
if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
{
trajectoryPositionsSubscriber.trajectoryPositionsWriters.Add(urdfJoint.transform.AddComponentIfNotExists <TrajectoryPositionsWriter>());
trajectoryPositionsSubscriber.jointNames.Add(urdfJoint.JointName);
foreach (UrdfJoint referenceRobotJoint in referenceRobot.GetComponentsInChildren <UrdfJoint>())
{
if (referenceRobotJoint.JointName == urdfJoint.JointName)
{
trajectoryPositionsSubscriber.trajectoryPositionsWriters[trajectoryPositionsSubscriber.trajectoryPositionsWriters.Count - 1].referenceJoint = referenceRobotJoint;
}
}
}
}
}
else
{
GetComponent <TrajectoryPositionsSubscriber>()?.trajectoryPositionsWriters.Clear();
GetComponent <TrajectoryPositionsSubscriber>()?.jointNames.Clear();
foreach (TrajectoryPositionsWriter writer in ghostRobot.GetComponentsInChildren <TrajectoryPositionsWriter>())
{
writer.transform.DestroyImmediateIfExists <TrajectoryPositionsWriter>();
}
}
}
// I do not have a need to publish commands, so I'm removing this capability
/*
* public void SetPublishJointCommand(bool publish)
* {
* if (publish)
* {
* JointCommandPublisher jointCommandPublisher = transform.AddComponentIfNotExists<JointCommandPublisher>();
* jointStatePublisher.JointStateReaders = new List<JointStateReader>();
*
* foreach (UrdfJoint urdfJoint in UrdfRobot.GetComponentsInChildren<UrdfJoint>())
* {
* if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
* jointStatePublisher.JointStateReaders.Add(urdfJoint.transform.AddComponentIfNotExists<JointStateReader>());
* }
* }
* else
* {
* GetComponent<JointStatePublisher>()?.JointStateReaders.Clear();
*
* foreach (JointStateReader reader in UrdfRobot.GetComponentsInChildren<JointStateReader>())
* reader.transform.DestroyImmediateIfExists<JointStateReader>();
* }
* }
*/
public void SetSubscribeJointCommands(bool subscribe)
{
if (subscribe)
{
JointCommandSubscriber jointCommandSubscriber = transform.AddComponentIfNotExists <JointCommandSubscriber>();
jointCommandSubscriber.JointCommandWriters = new List <JointCommandWriter>();
jointCommandSubscriber.JointNames = new List <string>();
foreach (UrdfJoint urdfJoint in UrdfRobot.GetComponentsInChildren <UrdfJoint>())
{
if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
{
jointCommandSubscriber.JointCommandWriters.Add(urdfJoint.transform.AddComponentIfNotExists <JointCommandWriter>());
jointCommandSubscriber.JointNames.Add(urdfJoint.JointName);
}
}
}
else
{
GetComponent <JointCommandSubscriber>()?.JointCommandWriters.Clear();
GetComponent <JointCommandSubscriber>()?.JointNames.Clear();
foreach (JointCommandWriter writer in UrdfRobot.GetComponentsInChildren <JointCommandWriter>())
{
writer.transform.DestroyImmediateIfExists <JointCommandWriter>();
}
}
}
void syncRobotJointStates(ref UrdfRobot robot)
{
IntPtr cmd_handle = NativeMethods.b3RequestActualStateCommandInit(pybullet, b3RobotId);
IntPtr status_handle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd_handle);
EnumSharedMemoryServerStatus status_type = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status_handle);
if (status_type == EnumSharedMemoryServerStatus.CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
for (int i = 0; i < robot.GetComponentsInChildren <UrdfJoint>().Length; i++)
{
var unityJoint = robot.GetComponentsInChildren <UrdfJoint>()[i];
if (!jointNames.Contains(unityJoint.JointName))
{
continue;
}
b3JointSensorState state = new b3JointSensorState();
NativeMethods.b3GetJointState(pybullet, status_handle, b3JointIds[i], ref state);
var diff = (float)state.m_jointPosition - unityJoint.GetPosition();
if (unityJoint.JointName.Contains("lh_") || unityJoint.JointName.Contains("rh_"))
{
continue;
//var candidates = from gi in glovesInfo
// where (gi.isRight && unityJoint.JointName.Contains("rh_")) || (unityJoint.JointName.Contains("lh_") && !gi.isRight)
// select gi;
//var gl = candidates.FirstOrDefault();
//if (gl.jointTFs == null) continue;
//// var gl = (unityJoint.JointName.Contains("rh_")) ? glovesInfo.Find(x => x.isRight) : glovesInfo.Find(x => !x.isRight);
//for (int j = 0; j < gl.thumbPrevSetpoints.Count; j++)
//{
// if (b3JointIds[i] == gl.handJointsIds[0][j])
// {
// diff = (float)(state.m_jointPosition - gl.thumbPrevSetpoints[j]);
// gl.thumbPrevSetpoints[j] = state.m_jointPosition;
// //Quaternion rot = Quaternion.AngleAxis(-diff * Mathf.Rad2Deg, new Vector3(0, 0, 1));// unityJoint.UnityJoint.axis);
// //unityJoint.transform.rotation = unityJoint.transform.rotation * rot;
// //unityJoint.GetComponentInChildren<Rigidbody>().transform.rotation = unityJoint.transform.rotation * rot;
// unityJoint.UpdateJointState(diff);
// break;
// }
//}
//continue;
}
unityJoint.UpdateJointState(diff);
}
}
else
{
Debug.LogWarning("Cannot get robot state");
}
}
private void Awake()
{
JointNames = new List <string>();
Velocities = new Dictionary <string, float>();
Positions = new Dictionary <string, float>();
Efforts = new Dictionary <string, float>();
foreach (UrdfJoint urdfJoint in _urdfRobot.GetComponentsInChildren <UrdfJoint>())
{
if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
{
JointNames.Add(urdfJoint.JointName);
Velocities[urdfJoint.JointName] = 0f;
Positions[urdfJoint.JointName] = 0f;
Efforts[urdfJoint.JointName] = 0f;
}
}
}
private static Robot ExportRobotData(this UrdfRobot urdfRobot)
{
Robot robot = new Robot();
robot.ConstructFromFile(urdfRobot.FilePath, urdfRobot.gameObject.name);
List <string> linkNames = new List <string>();
foreach (UrdfLink urdfLink in urdfRobot.GetComponentsInChildren <UrdfLink>())
{
//Link export
if (linkNames.Contains(urdfLink.name))
{
EditorUtility.DisplayDialog("URDF Export Error",
"URDF export failed. There are several links with the name " +
urdfLink.name + ". Make sure all link names are unique before exporting this robot.",
"Ok");
return(null);
}
robot.links.Add(urdfLink.ExportLinkData());
linkNames.Add(urdfLink.name);
//Joint export
UrdfJoint urdfJoint = urdfLink.gameObject.GetComponent <UrdfJoint>();
if (urdfJoint != null)
{
robot.joints.Add(urdfJoint.ExportJointData());
}
else if (!urdfLink.IsBaseLink)
{
//Make sure that links with no rigidbodies are still connected to the robot by a default joint
robot.joints.Add(UrdfJoint.ExportDefaultJoint(urdfLink.transform));
}
}
robot.materials = UrdfMaterial.Materials.Values.ToList();
robot.plugins = urdfRobot.GetComponentInChildren <UrdfPlugins>().ExportPluginsData();
return(robot);
}
void setupRobotJoints()
{
b3JointInfo jointInfo = new b3JointInfo();
var b3JointsNum = NativeMethods.b3GetNumJoints(pybullet, b3RobotId);
b3JointNames = new List <string>();
jointNames = new List <string>();
var urdfJoints = urdfRobot.GetComponentsInChildren <UrdfJoint>();
b3JointIds = new List <int>();
freeJoints = new List <int>();
excludeFromIkJoints = new List <int>();
var cmd = NativeMethods.b3JointControlCommandInit2(pybullet, b3RobotId, (int)EnumControlMode.CONTROL_MODE_POSITION_VELOCITY_PD);
for (int i = 0; i < b3JointsNum; i++)
{
NativeMethods.b3GetJointInfo(pybullet, b3RobotId, i, ref jointInfo);
b3JointNames.Add(jointInfo.m_jointName);
if (jointInfo.m_jointType == 0)//JointType.eRevoluteType)
{
freeJoints.Add(i);
}
if (jointInfo.m_jointName.Contains("rh_") || jointInfo.m_jointName.Contains("lh_") || jointInfo.m_jointName.Contains("head"))
{
excludeFromIkJoints.Add(i);
}
//if (jointInfo.m_jointName.Contains("lh"))
//{
// setJointPosition(ref cmd, i, 0);
//}
}
NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
foreach (var j in urdfJoints)
{
if (b3JointNames.Contains(j.JointName))
{
b3JointIds.Add(b3JointNames.IndexOf(j.JointName));
jointNames.Add(j.JointName);
}
else
{
Debug.LogWarning("pybullet doesnt know about " + j.JointName);
}
}
headJointIds = new List <int>
{
b3JointIds.ElementAt(jointNames.IndexOf("head_axis0")),
b3JointIds.ElementAt(jointNames.IndexOf("head_axis2")),
b3JointIds.ElementAt(jointNames.IndexOf("head_axis1"))
};
wristJoints = new List <int>
{
b3JointIds.ElementAt(jointNames.IndexOf("wrist_right_axis0")),
b3JointIds.ElementAt(jointNames.IndexOf("wrist_right_axis1")),
b3JointIds.ElementAt(jointNames.IndexOf("wrist_right_axis2"))
};
}
