public string b3GetJointName(int jointId, int bodyId)
{
b3JointInfo ji = new b3JointInfo();
NativeMethods.b3GetJointInfo(pybullet, bodyId, jointId, ref ji);
return(ji.m_jointName);
}
//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("THJ"))
// {
// 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");
// }
//}
//void trackHead()
//{
// var eulerAngles = cam.transform.rotation.eulerAngles;
// double pitch, roll, yaw;
// roll = eulerAngles.x;
// pitch = eulerAngles.y - camYOffset;
// yaw = eulerAngles.z;
// Debug.Log(clipAngle(yaw) * Mathf.Deg2Rad);
// IntPtr cmd = NativeMethods.b3JointControlCommandInit2(pybullet, b3RobotId, 2);
// setJointPosition(ref cmd, headJointIds[0], clipAngle(roll) * Mathf.Deg2Rad);
// setJointPosition(ref cmd, headJointIds[1], clipAngle(yaw) * Mathf.Deg2Rad);
// setJointPosition(ref cmd, headJointIds[2], clipAngle(pitch) * Mathf.Deg2Rad);
// var status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
// lastUpdate = Time.time * 1000;
//}
public void SetJointPosition(ref IntPtr cmd, int bodyId, int jointIndex, double targetPositionRad)
{
b3JointInfo ji = new b3JointInfo();
NativeMethods.b3GetJointInfo(pybullet, bodyId, jointIndex, ref ji);
NativeMethods.b3JointControlSetDesiredPosition(cmd, ji.m_qIndex, targetPositionRad);
NativeMethods.b3JointControlSetKp(cmd, ji.m_uIndex, 0.1);
NativeMethods.b3JointControlSetDesiredVelocity(cmd, ji.m_uIndex, 0);
NativeMethods.b3JointControlSetKd(cmd, ji.m_uIndex, 1.0);
NativeMethods.b3JointControlSetMaximumForce(cmd, ji.m_uIndex, 100000.0);
}
//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"))
// };
// //handJoints = new List<List<int>>(){
// // new List<int> {
// // b3JointIds.ElementAt(b3JointNames.IndexOf("rh_THJ5")),
// // b3JointIds.ElementAt(b3JointNames.IndexOf("rh_THJ4")),
// // b3JointIds.ElementAt(b3JointNames.IndexOf("rh_THJ3")),
// // b3JointIds.ElementAt(b3JointNames.IndexOf("rh_THJ2"))
// // //b3JointIds.ElementAt(jointNames.IndexOf("rh_THJ1"))
// // },
// // new List<int> {
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_FFJ3")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_FFJ2")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_FFJ1"))
// // },
// // new List<int> {
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_MFJ3")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_MFJ2")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_MFJ1"))
// // },
// // new List<int> {
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_RFJ3")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_RFJ2")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_RFJ1"))
// // },
// // new List<int> {
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_LFJ3")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_LFJ2")),
// // b3JointIds.ElementAt(jointNames.IndexOf("rh_LFJ1"))
// // }
// //};
// //thumbPrevSetpoints = new List<double>(new double[handJoints[0].Count]);
//}
public int b3GetLinkId(string name, int bodyId)
{
b3JointInfo ji = new b3JointInfo();
var b3JointsNum = NativeMethods.b3GetNumJoints(pybullet, bodyId);
for (int i = 0; i < b3JointsNum; i++)
{
NativeMethods.b3GetJointInfo(pybullet, bodyId, i, ref ji);
if (ji.m_linkName == name)
{
return(i);
}
}
Debug.LogError("Could not find link id with name " + name);
return(-1);
}
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"))
};
}
//TODO deal with loading status
public int LoadUrdfAndGetBodyIdx(string modelName, Vector3 p, Quaternion qq)
{
IntPtr cmd = NativeMethods.b3LoadUrdfCommandInit(_native, modelName);
//IntPtr cmd = NativeMethods.b3LoadMJCFCommandInit(_native, modelName);
NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, qq.x, qq.y, qq.z, qq.w);
NativeMethods.b3LoadUrdfCommandSetStartPosition(cmd, p.x, p.y, p.z);
IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(_native, cmd);
int bodyIdx = NativeMethods.b3GetStatusBodyIndex(status);
Debug.Log("body index " + bodyIdx);
cmd = NativeMethods.b3InitRequestVisualShapeInformation(_native, bodyIdx);
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(_native, cmd);
EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
if (statusType == EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED ||
statusType == EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_UPDATE_COMPLETED)
{
b3VisualShapeInformation visuals = new b3VisualShapeInformation();
NativeMethods.b3GetVisualShapeInformation(_native, ref visuals);
Debug.Log("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes);
int numVisualShapes = visuals.m_numVisualShapes;
System.IntPtr visualPtr = visuals.m_visualShapeData;
//robot.bodies = new Body[visuals.m_numVisualShapes];
for (int s = 0; s < visuals.m_numVisualShapes; s++)
{
b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData));
visualPtr = new IntPtr(visualPtr.ToInt64() + (Marshal.SizeOf(typeof(b3VisualShapeData))));
Vector3 scale;
scale.x = (float)visual.m_dimensions[0];
scale.y = (float)visual.m_dimensions[1];
scale.z = (float)visual.m_dimensions[2];
Debug.Log("visual.m_visualGeometryType =" + (eUrdfGeomTypes)visual.m_visualGeometryType +
" linkIndex=" + visual.m_linkIndex + " id=" + visual.m_objectUniqueId);
Vector3 pos;
pos.x = (float)visual.m_localVisualFrame[0];
pos.y = (float)visual.m_localVisualFrame[1];
pos.z = (float)visual.m_localVisualFrame[2];
Quaternion rot;
rot.x = (float)visual.m_localVisualFrame[3];
rot.y = (float)visual.m_localVisualFrame[4];
rot.z = (float)visual.m_localVisualFrame[5];
rot.w = (float)visual.m_localVisualFrame[6];
//robot.bodies[s] = CreateShape(visual, "cube", pos, rot, scale);
if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH)
{
Debug.Log("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName);
}
}
}
int numJoints = NativeMethods.b3GetNumJoints(_native, bodyIdx);
Dictionary <string, int> jointNameToId = new Dictionary <string, int>();
for (int i = 0; i < numJoints; i++)
{
b3JointInfo jointInfo = new b3JointInfo();
NativeMethods.b3GetJointInfo(_native, bodyIdx, i, ref jointInfo);
jointNameToId.Add(jointInfo.m_jointName, i);
Debug.Log("Joint name " + jointInfo.m_jointName + " Link name " + jointInfo.m_linkName);
}
//NativeMethods.collisionshape(_native, bodyIdx);
return(bodyIdx);
}
