// Start is called before the first frame update
void Start()
{
bb = GameObject.Find("BulletBridge").GetComponent <BulletBridge>();
while (!bb.isInitialized)
{
StartCoroutine(bb.WaitForConnection());
//bb = GameObject.Find("BulletBridge").GetComponent<BulletBridge>();
}
pybullet = bb.GetPhysicsServerPtr();
Debug.Log("Connected " + name + " to bullet server.");
BulletBridge.MakeKinematic(GetComponentsInChildren <Rigidbody>());
if (resetPose)
{
var newPos = baseFrame.position + baseFrame.InverseTransformPoint(translation);
var newOrn = rotation + baseFrame.rotation.eulerAngles;
transform.SetPositionAndRotation(newPos, Quaternion.Euler(newOrn));
b3BodyId = bb.LoadURDF(Application.dataPath + urdfPath, newPos, Quaternion.Euler(newOrn), useFixedBase ? 1 : 0);
}
else
{
b3BodyId = bb.LoadURDF(Application.dataPath + urdfPath, transform.position, transform.rotation, useFixedBase ? 1 : 0);
}
bb.AddGameObject(gameObject, b3BodyId);
var childBodies = GetComponentsInChildren <BulletBody>();
isInitialized = true;
foreach (var child in childBodies)
{
child.enabled = true;
}
}
void trackHead()
{
var eulerAngles = cameraTF.rotation.eulerAngles;
double pitch, roll, yaw;
roll = eulerAngles.x;
pitch = eulerAngles.y - camYOffset;
yaw = eulerAngles.z;
//Debug.Log(BulletBridge.ClipAngle(yaw) * Mathf.Deg2Rad);
IntPtr cmd = NativeMethods.b3JointControlCommandInit2(pybullet, b3RobotId, 2);
bb.SetJointPosition(ref cmd, b3RobotId, headJointIds[0], BulletBridge.ClipAngle(roll) * Mathf.Deg2Rad);
bb.SetJointPosition(ref cmd, b3RobotId, headJointIds[1], BulletBridge.ClipAngle(yaw) * Mathf.Deg2Rad);
bb.SetJointPosition(ref cmd, b3RobotId, headJointIds[2], BulletBridge.ClipAngle(pitch) * Mathf.Deg2Rad);
var status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
lastUpdate = Time.time * 1000;
}
// Start is called before the first frame update
void Start()
{
bb = GameObject.Find("BulletBridge").GetComponent <BulletBridge>();
while (!bb.isInitialized)
{
bb.WaitForConnection();
}
pybullet = bb.GetPhysicsServerPtr();
Debug.Log("Connected " + name + " to bullet server.");
BulletBridge.MakeKinematic(GetComponentsInChildren <Rigidbody>());
resetRobotPose();
urdfRobot = GetComponent <UrdfRobot>();
var robotPath = Application.dataPath + urdfPath;
b3RobotId = bb.LoadURDF(robotPath, transform.position, transform.rotation, 1);
bb.AddGameObject(gameObject, b3RobotId);
setupRobotJoints();
if (trackIK)
{
IKTargets = new List <IKTarget>()
{
new IKTarget(rightHandTarget, bb.b3GetLinkId("rh_palm", b3RobotId), bb.GetJointKinematicChain(b3RobotId, "rh_palm", "shoulder_right_link1")),
new IKTarget(leftHandTarget, bb.b3GetLinkId("lh_palm", b3RobotId), bb.GetJointKinematicChain(b3RobotId, "lh_palm", "shoulder_left_link1"))
};
}
lastUpdate = Time.time * 1000;
camYOffset = cameraTF.rotation.eulerAngles.y;
var cmd = NativeMethods.b3InitSyncBodyInfoCommand(pybullet);
var status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
}
