/// <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()
{
MoverServiceRequest request = new MoverServiceRequest
{
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 Pose
{
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 Pose
{
position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To <FLU>(),
orientation = pickOrientation.To <FLU>()
}
};
ros.Send(rosJointPublishTopicName, request);
}
/// <summary>
/// Publish the robot's current joint configuration, the m_Target object's
/// position and rotation, and the m_Target placement for the m_Target object's
/// position and rotation.
/// Includes conversion from Unity coordinates to ROS coordinates, Forward Left Up
/// </summary>
public void PublishJoints()
{
var request = new MoverServiceRequest
{
joints_input = new NiryoMoveitJointsMsg(),
pick_pose = new PoseMsg
{
position = (m_Target.transform.position + m_PickPoseOffset).To <FLU>(),
// The hardcoded x/z angles assure that the gripper is always positioned above the m_Target cube before grasping.
orientation = Quaternion.Euler(90, m_Target.transform.eulerAngles.y, 0).To <FLU>()
},
place_pose = new PoseMsg
{
position = (m_TargetPlacement.transform.position + m_PickPoseOffset).To <FLU>(),
orientation = m_PickOrientation.To <FLU>()
}
};
for (var i = 0; i < k_NumRobotJoints; i++)
{
request.joints_input.joints[i] = m_JointArticulationBodies[i].jointPosition[0];
}
m_Ros.Publish(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()
{
MoverServiceRequest request = new MoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new RosMessageTypes.Geometry.Pose
{
position = new Point(
target.transform.position.z,
-target.transform.position.x,
// Add pick pose offset to position the gripper above target to avoid collisions
target.transform.position.y + pickPoseOffset
),
// Orientation is hardcoded for this example so the gripper is always directly above the target object
orientation = pickOrientation
};
// Place Pose
request.place_pose = new RosMessageTypes.Geometry.Pose
{
position = new Point(
targetPlacement.transform.position.z,
-targetPlacement.transform.position.x,
// Use the same pick pose offset so the target cube can be seen dropping into position
targetPlacement.transform.position.y + pickPoseOffset
),
// Orientation is hardcoded for this example so the gripper is always directly above the target object
orientation = pickOrientation
};
ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
}
public void PublishJoints(Vector3 targetPos, Quaternion targetRot)
{
MoverServiceRequest request = new MoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new RosMessageTypes.Geometry.Pose
{
position = (targetPos + pickPoseOffset).To <FLU>(),
orientation = Quaternion.Euler(90, targetRot.eulerAngles.y, 0).To <FLU>()
};
// Place Pose
request.place_pose = new RosMessageTypes.Geometry.Pose
{
position = (goal.position + placePoseOffset).To <FLU>(),
orientation = pickOrientation.To <FLU>()
};
ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
}
/// <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()
{
MoverServiceRequest request = new MoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new RosMessageTypes.Geometry.Pose
{
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 RosMessageTypes.Geometry.Pose
{
position = (targetPlacement.transform.position + pickPoseOffset).To <FLU>(),
orientation = pickOrientation.To <FLU>()
};
ros.SendServiceMessage <MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
}
