/// <summary>
/// Publish the robot's current joint configuration, the target object's
/// position and rotation, and the target placement for the target object's
/// position and rotation.
///
/// Includes conversion from Unity coordinates to ROS coordinates, Forward Left Up
/// </summary>
public void PublishJoints()
{
MMoverServiceRequest request = new MMoverServiceRequest
{
joints_input =
{
joint_00 = jointArticulationBodies[0].xDrive.target,
joint_01 = jointArticulationBodies[1].xDrive.target,
joint_02 = jointArticulationBodies[2].xDrive.target,
joint_03 = jointArticulationBodies[3].xDrive.target,
joint_04 = jointArticulationBodies[4].xDrive.target,
joint_05 = jointArticulationBodies[5].xDrive.target
},
pick_pose = new MPose
{
position = (target.transform.position + PICK_POSE_OFFSET).To <FLU>(),
// The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
},
place_pose = new MPose
{
position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To <FLU>(),
orientation = pickOrientation.To <FLU>()
}
};
ros.Send(rosJointPublishTopicName, request);
}
/// <summary>
/// Create a new MoverServiceRequest with the current values of the robot's joint angles,
/// the target cube's current position and rotation, and the targetPlacement position and rotation.
///
/// Call the MoverService using the ROSConnection and if a trajectory is successfully planned,
/// execute the trajectories in a coroutine.
/// </summary>
public void PublishJoints()
{
MMoverServiceRequest request = new MMoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new MPose
{
position = (target.transform.position + pickPoseOffset).To <FLU>(),
// The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
};
// Place Pose
request.place_pose = new MPose
{
position = (targetPlacement.transform.position + pickPoseOffset).To <FLU>(),
orientation = pickOrientation.To <FLU>()
};
ros.SendServiceMessage <MMoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
}
