public void SetPos(float angle0, float angle1, float angle2, float angle3, float angle4, float angle5, bool gripped)
{
if (config.Kinematic)
{
float deltaangle0 = kinematicanglerange0.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle0();
float deltaangle1 = kinematicanglerange1.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle1();
float deltaangle2 = kinematicanglerange2.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle2();
kinematicanglerange0.SetTarget(angle0);
kinematicanglerange1.SetTarget(angle1);
kinematicanglerange2.SetTarget(-90 + angle2);
deltaangle0 = kinematicanglerange0.Delta(deltaangle0, kinematicanglerange0.GetTarget());
deltaangle1 = kinematicanglerange0.Delta(deltaangle1, kinematicanglerange1.GetTarget());
deltaangle2 = kinematicanglerange0.Delta(deltaangle2, kinematicanglerange2.GetTarget());
chassis.GetComponent <chassismanipulator2>().SetKinematicTargetDeltaAngles(deltaangle0, deltaangle1, deltaangle2);
}
else
{
DriveJoint drive0 = chassis.GetComponent <chassismanipulator2>().drive;
DriveJoint drive1 = lever.GetComponent <levermanipulator2>().drive;
DriveJoint drive2 = holder1.GetComponent <holder1manipulator2>().drive;
DriveJoint drive3 = holder2.GetComponent <holder2manipulator2>().drive;
drive0.AngleRange.SetTarget(angle0);
drive3.AngleRange.SetTarget(angle1);
drive2.AngleRange.SetTarget(drive3.AngleRange.GetTarget() + drive1.AngleRange.GetTarget());
drive1.AngleRange.SetTarget(-90 + angle2);
drive2.AngleRange.SetTarget(drive3.AngleRange.GetTarget() + drive1.AngleRange.GetTarget());
}
if (config.FingerDown)
{
capture.SetPos(0, GetPos1() + GetPos2() - 180, angle5);
}
else
{
capture.SetPos(angle3, angle4, angle5);
}
SetGripper(gripped);
}
