public void PlanCartesianPath(int _instanceID, float _sockelOffsetX, float _sockelOffsetY, float _sockelOffsetZ, bool _useSpecialBoxGeometry)
{
instanceID = _instanceID;
_sockelOffsetY /= 100;
/*
* MOVE OBSTACLES BASED ON SOCKEL POSITION OFFSET
*/
MoveCollisionObjects(_sockelOffsetX, _sockelOffsetY, _sockelOffsetZ, _useSpecialBoxGeometry);
/*
* ADJUST POSITIONS ON PATH BASED ON SOCKEL POSITION OFFSET
*/
posesOnPath = new Pose[]
{
new Pose(new Point(aboveBox.x - _sockelOffsetZ, aboveBox.y - _sockelOffsetX, aboveBox.z - _sockelOffsetY),
orientation),
new Pose(new Point(inBox.x - _sockelOffsetZ, inBox.y - _sockelOffsetX, inBox.z - _sockelOffsetY),
orientation),
new Pose(new Point(aboveBox.x - _sockelOffsetZ, aboveBox.y - _sockelOffsetX, aboveBox.z - _sockelOffsetY + 0.20),
orientation),
new Pose(new Point(aboveInducter.x - _sockelOffsetZ, aboveInducter.y - _sockelOffsetX, aboveInducter.z - _sockelOffsetY),
orientation),
new Pose(new Point(onInducter.x - _sockelOffsetZ, onInducter.y - _sockelOffsetX, onInducter.z - _sockelOffsetY),
orientation),
new Pose(new Point(aboveInducter.x - _sockelOffsetZ, aboveInducter.y - _sockelOffsetX, aboveInducter.z - _sockelOffsetY),
orientation),
new Pose(new Point(aboveBox.x - _sockelOffsetZ, aboveBox.y - _sockelOffsetX, aboveBox.z - _sockelOffsetY),
orientation),
};
/*
* PLAN IN CARTESIAN SPACE
*/
var getCartesianPathRequest = new GetCartesianPathRequest();
getCartesianPathRequest.header.Update();
getCartesianPathRequest.group_name = "panda_arm";
var robotJointState = new JointState
{
header = new Header(),
name = jointGroup.jointNames,
position = initialPosition
};
getCartesianPathRequest.start_state.joint_state = robotJointState;
getCartesianPathRequest.link_name = "panda_link8";
getCartesianPathRequest.waypoints = posesOnPath;
getCartesianPathRequest.avoid_collisions = true;
getCartesianPathRequest.max_step = 0.01;
rosSocket.CallService <GetCartesianPathRequest, GetCartesianPathResponse>("/compute_cartesian_path",
ServiceCallHandler, getCartesianPathRequest);
OnServiceReceived.WaitOne();
OnServiceReceived.Reset();
}
/// <summary> Setter constructor. </summary>
public GetCartesianPath(GetCartesianPathRequest request)
{
Request = request;
Response = new GetCartesianPathResponse();
}
/// <summary> Empty constructor. </summary>
public GetCartesianPath()
{
Request = new GetCartesianPathRequest();
Response = new GetCartesianPathResponse();
}